% puma560


format long
syms d1 d2 d3 d4 d5 d6 
syms theta1 theta2 theta3 theta4 theta5 theta6 
syms theta_i theta_i_1 alpha_i_1 alpha_i d_i a_i_1
syms a1 a2 a3 a4 a5 a6
syms alpha1 alpha2 alpha3 alpha4 alpha5 alpha6
syms r11 r12 r13 r21 r22 r23 r31 r32 r33 Px Py Pz
syms T_i
%机器人学PUMA560机械臂参数
d1 = 0;
d2 = 0;
d3 = 0.149;
d4 = 0.433;
d5 = 0;
d6 = 0;
%连杆长度
a0 = 0;
a1 = 0;
a2 = 0.432;
a3 = 0.02;
a4 = 0;
a5 = 0;
%连杆扭角
alpha0 = 0;
alpha1 = -pi/2;
alpha2 = 0;
alpha3 = -pi/2;
alpha4 = pi/2;
alpha5 = -pi/2;
L1=Link([0     d1       a0       alpha0     ],'modified');%设置连杆参数，注：使用的是改进型DH参数，所以最后参数为'modified'
L2=Link([0     d2       a1       alpha1     ],'modified');
L3=Link([0     d3       a2        alpha2     ],'modified');
L4=Link([0     d4       a3       alpha3     ],'modified');
L5=Link([0     d5       a4       alpha4     ],'modified');
L6=Link([0     d6       a5       alpha5     ],'modified');
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','RobotArm');%将不同连杆链接起来
robot.plot([pi/3,pi/4,pi/5,pi/3,pi/4,pi/5]);%输出机器人模型，后面的六个角为输出时的theta姿态,用弧度制
robot.display();%打印出机器人D-H参数表
T=robot.fkine([pi/3,pi/4,pi/5,pi/3,pi/4,pi/5])%求机械臂正解
robot.teach%机械臂示教
%使用改进型DH法，根据机器人学中对PUMA560机械臂建模方法实现运动学正解